#ifndef __DALELIB_ROBOTPART_H__
#define __DALELIB_ROBOTPART_H__

//#define dDOUBLE

#include <dalelib/maths.h>
#include <dalelib/graphics.h>
#include <dalelib/graphics/shader.h>
#include <ode/ode.h>


namespace dalelib
{

class RobotPart
{
protected:

	dBodyID _body;
	dGeomID _geom;

	float _touch;
	v3 _size;
	m4 _global_mat;
	dMass _mass;

	v3 _cached_pos;
	m3 _cached_rot;
	int _id;

	GLuint	_display_list_id;

public:

	RobotPart(dWorldID _World, dSpaceID _Space);
	~RobotPart();
	void reset();
	float getTouch() { return _touch; }
	void setTouch(float _Value) { _touch = _Value; }
	int& getID() { return _id; }
	const dBodyID getBody() { return _body; }
	const dGeomID getGeom() { return _geom; }
	const v3 getSize() { return _size; }

	v3 getPosition() { const dReal *pos = dBodyGetPosition(_body); return v3(pos[0], pos[1], pos[2]); }
	void setPosition(const v3 _Pos) { dBodySetPosition(_body,_Pos[0],_Pos[1],_Pos[2]); }
	void translate(const v3 _Pos) { const dReal *pos = dBodyGetPosition(_body); dBodySetPosition(_body,pos[0]+_Pos[0],pos[1]+_Pos[1],pos[2]+_Pos[2]); }
	m3 getRotation() { const dReal *rot = dBodyGetRotation(_body); return m3(rot[0],rot[4],rot[8], rot[1],rot[5],rot[9], rot[2],rot[6],rot[10]); }
	void setRotation(m3 _Rot)
	{
		dReal f[12];
		f[ 0] = _Rot[0]; f[ 1] = _Rot[3]; f[ 2] = _Rot[6]; f[ 3] = 0.f;
		f[ 4] = _Rot[1]; f[ 5] = _Rot[4]; f[ 6] = _Rot[7]; f[ 7] = 0.f;
		f[ 8] = _Rot[2]; f[ 9] = _Rot[5]; f[10] = _Rot[8]; f[11] = 0.f;
		dBodySetRotation(_body,f);
	}
	v3 getStart() { v3 pos = getPosition(), dir = getZAxis(); return pos - dir*(_size[2]*0.5f); }
	v3 getEnd() { v3 pos = getPosition(), dir = getZAxis(); return pos + dir*(_size[2]*0.5f); }
	v3 getLinearVelocity() { const dReal *vel = dBodyGetLinearVel(_body); return v3(vel[0], vel[1], vel[2]); }
	v3 getAngularVelocity() { const dReal *vel = dBodyGetAngularVel(_body); return v3(vel[0], vel[1], vel[2]); }
	v3 getXAxis() { const dReal *rot = dBodyGetRotation(_body); return v3(rot[0],rot[4],rot[8]).unit(); }
	v3 getYAxis() { const dReal *rot = dBodyGetRotation(_body); return v3(rot[1],rot[5],rot[9]).unit(); }
	v3 getZAxis() { const dReal *rot = dBodyGetRotation(_body); return v3(rot[2],rot[6],rot[10]).unit(); }

	m4& getGlobalMat() { return _global_mat; }

	void update();
	void draw();

};

//*************************************************************
struct RobotCapsule : public RobotPart
{
	RobotCapsule(dWorldID _World, dSpaceID _Space, const float _Radius, const v3 _Position0, const v3 _Position1, const float _Density);
	void createCapsule(const v3 _Size, int _Stacks=16, int _Slices=16);
};

//*************************************************************
struct RobotBox : public RobotPart
{
	RobotBox(dWorldID _World, dSpaceID _Space, const float _Radius, const v3 _Position0, const v3 _Position1, const float _Density);
	void createBox(const v3 _Size);
};


} // namespace dalelib

#endif
